package com.jhc.drone.communication.mission;


import com.jhc.drone.communication.enums.AltTypeEnum;
import com.jhc.drone.communication.mavlink.MAVLinkPacket;
import com.jhc.drone.communication.mavlink.common.msg_command_int;
import com.jhc.drone.communication.mavlink.common.msg_mission_clear_all;
import com.jhc.drone.communication.mavlink.common.msg_mission_count;
import com.jhc.drone.communication.mavlink.common.msg_mission_item_int;
import com.jhc.drone.communication.mavlink.enums.MAV_CMD;
import com.jhc.drone.communication.mavlink.enums.MAV_FRAME;
import com.jhc.drone.communication.mavlink.enums.MAV_MISSION_TYPE;
import com.jhc.drone.communication.vo.CommandVo;
import com.jhc.drone.communication.vo.PointVo;
import lombok.extern.slf4j.Slf4j;

import java.util.ArrayList;
import java.util.List;
import java.util.concurrent.atomic.AtomicInteger;

/**
 * @author lcp
 * @date 2025年01月2025/1/8 15:05
 * @describe 任务流程
 */
@Slf4j
public class MissionBuilder {

    // first
    public byte[] setHome(CommandVo vo) {
        msg_command_int commandInt = new msg_command_int(vo);
        commandInt.command = MAV_CMD.MAV_CMD_DO_SET_HOME;
        commandInt.x = 0; //400808070
        commandInt.y = 0;
        commandInt.z = 40;
        commandInt.target_system = (short) vo.getSysId();
        commandInt.target_component = (short) vo.getComId();
        commandInt.sysid = vo.getSysId();
        commandInt.compid = vo.getComId();
        commandInt.param4 = Float.NaN;
        // log.info("-------set home:{}-------", new Gson().toJson(commandInt));
        System.out.println("set home--->" + commandInt);
        MAVLinkPacket pack = commandInt.pack();
        return pack.encodePacket();
    }

    // second
    public byte[] clearAllMission(CommandVo vo) {
        msg_mission_clear_all clearAll = new msg_mission_clear_all();
        clearAll.sysid = vo.getSysId();
        clearAll.compid = vo.getComId();
        clearAll.target_system = (short) vo.getSysId();
        clearAll.target_component = (short) vo.getComId();
        clearAll.mission_type = MAV_MISSION_TYPE.MAV_MISSION_TYPE_MISSION;
        System.out.println("clearAll--->" + clearAll);

        //log.info("-------clearAll:{}-------", new Gson().toJson(clearAll));
        MAVLinkPacket pack = clearAll.pack();
        return pack.encodePacket();
    }

    // third
    public byte[] missionCount(CommandVo vo) {
        msg_mission_count missionCount = new msg_mission_count();
        missionCount.sysid = vo.getSysId();
        missionCount.compid = vo.getComId();
        missionCount.target_system = (short) vo.getSysId();
        missionCount.target_component = (short) vo.getComId();
        missionCount.count = 1 + (3 * vo.getMissionVo().getPoints().size()); // the num of remain cmd
        missionCount.mission_type = MAV_MISSION_TYPE.MAV_MISSION_TYPE_MISSION;
        System.out.println("missionCount--->" + missionCount);
        // log.info("-------missionCount:{}-------", new Gson().toJson(missionCount));
        MAVLinkPacket pack = missionCount.pack();
        return pack.encodePacket();
    }

    /**
     * @describe 描述：
     * target_system: 50, target_component: 190, command: TAKEOFF,
     * mission_type: MISSION, seq: 0, frame: GLOBAL,
     * current: 0, autocontinue : 1, param1: 0, param2: 0, param3: 0,
     * param4: NaN, x: 400808070, y: 1165726009, z: 50
     */

    public byte[] takeoff(CommandVo vo) {
        msg_mission_item_int takeoffItem = new msg_mission_item_int();

        takeoffItem.command = MAV_CMD.MAV_CMD_NAV_TAKEOFF;
        takeoffItem.sysid = vo.getSysId();
        takeoffItem.compid = vo.getComId();
        takeoffItem.target_system = (short) vo.getSysId();
        takeoffItem.target_component = 190;
        takeoffItem.seq = 0;
        takeoffItem.frame = MAV_FRAME.MAV_FRAME_GLOBAL;
        takeoffItem.current = 0;
        takeoffItem.autocontinue = 1;
        takeoffItem.param4 = Float.NaN;
        //takeoffItem.z = 50;  //takeoffItem.x = 400808070;//takeoffItem.y = 1165726010;
        takeoffItem.z = vo.getMissionVo().getAltitude() != 0 ? vo.getMissionVo().getAltitude() : 50;
        takeoffItem.mission_type = MAV_MISSION_TYPE.MAV_MISSION_TYPE_MISSION;
        System.out.println("takeoff--->" + takeoffItem);
        MAVLinkPacket takeoffPacket = takeoffItem.pack();
        return takeoffPacket.encodePacket();
    }

    public byte[] waypoint(CommandVo vo, PointVo point, int seq) {
        msg_mission_item_int msgMissionItemInt1 = new msg_mission_item_int();
        msgMissionItemInt1.command = MAV_CMD.MAV_CMD_NAV_WAYPOINT;
        msgMissionItemInt1.sysid = vo.getSysId();
        msgMissionItemInt1.compid = vo.getComId();
        msgMissionItemInt1.target_system = (short) vo.getSysId();
        msgMissionItemInt1.target_component = 190;
        msgMissionItemInt1.seq = seq;
        msgMissionItemInt1.frame = point.getAltType() == AltTypeEnum.RELATIVE ? (short) MAV_FRAME.MAV_FRAME_LOCAL_NED : (short) MAV_FRAME.MAV_FRAME_GLOBAL;
        msgMissionItemInt1.current = 0;
        msgMissionItemInt1.autocontinue = 1;
        msgMissionItemInt1.y = (int) (point.getLon() * 10000000);
        msgMissionItemInt1.x = (int) (point.getLat() * 10000000);
        msgMissionItemInt1.z = point.getAltitude();
        msgMissionItemInt1.param4 = Float.NaN;
        //msgMissionItemInt1.param4 = 0.5f; //机头朝向的度数(没有值就传NaN)
        // msgMissionItemInt.param1 = 5; // 到点停留时间
        msgMissionItemInt1.mission_type = MAV_MISSION_TYPE.MAV_MISSION_TYPE_MISSION;
        System.out.println("waypoint--->" + msgMissionItemInt1);
        return msgMissionItemInt1.pack().encodePacket();
    }

    public byte[] pointAction(CommandVo vo, int seq) {
        msg_mission_item_int msgMissionItemInt2 = new msg_mission_item_int();
        msgMissionItemInt2.command = MAV_CMD.MAV_CMD_DO_RISING_MODE_POINT_ACTION;
        msgMissionItemInt2.sysid = vo.getSysId();
        msgMissionItemInt2.compid = vo.getComId();
        msgMissionItemInt2.target_system = (short) vo.getSysId();
        msgMissionItemInt2.target_component = 190;
        msgMissionItemInt2.seq = seq;
        msgMissionItemInt2.frame = MAV_FRAME.MAV_FRAME_MISSION;
        msgMissionItemInt2.current = 0;
        msgMissionItemInt2.autocontinue = 1;
        msgMissionItemInt2.param1 = 3; // 变高方式：1-优先变高，2-到点变高，3-安全变高，4-均匀变高，5-仿地变高
        msgMissionItemInt2.param2 = 0; // 1-投放，2-喷洒，3-拍摄，4-环绕
        msgMissionItemInt2.param3 = Float.NaN;
        msgMissionItemInt2.param4 = Float.NaN;
        msgMissionItemInt2.z = Float.NaN;

        // msgMissionItemInt2.param3 = 16; // 当param2=1时候，param3为投放通道（1~16），当param2=2喷洒时候，param3为喷洒通道（1~16）
        // msgMissionItemInt2.param4 = 143; // 当param2=4时候，param4为环绕半径，
        // msgMissionItemInt2.x = 143; // 当param2=4时候，x为环绕时间
        // msgMissionItemInt2.y = 10; // 当param2=4时候，y为环绕圈数量
        msgMissionItemInt2.mission_type = MAV_MISSION_TYPE.MAV_MISSION_TYPE_MISSION;
        System.out.println("point action--->" + msgMissionItemInt2);
        return msgMissionItemInt2.pack().encodePacket();
    }

    public byte[] changeSpeed(CommandVo vo, PointVo point, int seq) {
        msg_mission_item_int msgMissionItemInt3 = new msg_mission_item_int();
        msgMissionItemInt3.command = MAV_CMD.MAV_CMD_DO_CHANGE_SPEED;
        msgMissionItemInt3.sysid = vo.getSysId();
        msgMissionItemInt3.compid = vo.getComId();
        msgMissionItemInt3.target_system = (short) vo.getSysId();
        msgMissionItemInt3.target_component = 190;
        msgMissionItemInt3.seq = seq;
        msgMissionItemInt3.frame = MAV_FRAME.MAV_FRAME_MISSION;
        msgMissionItemInt3.current = 0;
        msgMissionItemInt3.autocontinue = 1;
        msgMissionItemInt3.param1 = 1;
        msgMissionItemInt3.param2 = point.getSpeed(); // 速度值
        msgMissionItemInt3.param3 = -1;
        msgMissionItemInt3.mission_type = MAV_MISSION_TYPE.MAV_MISSION_TYPE_MISSION;
        System.out.println("change speed--->" + msgMissionItemInt3);
        return msgMissionItemInt3.pack().encodePacket();
    }

    // fifth
    public List<byte[]> missionPoint(CommandVo vo) {
        List<byte[]> bytes = new ArrayList<>();
        AtomicInteger seq = new AtomicInteger(0);
        msg_mission_item_int takeoffItem = new msg_mission_item_int();

        takeoffItem.command = MAV_CMD.MAV_CMD_NAV_TAKEOFF;
        takeoffItem.sysid = vo.getSysId();
        takeoffItem.compid = vo.getComId();
        takeoffItem.target_system = (short) vo.getSysId();
        takeoffItem.target_component = 190;
        takeoffItem.seq = seq.getAndIncrement();
        takeoffItem.frame = MAV_FRAME.MAV_FRAME_GLOBAL;
        takeoffItem.current = 0;
        takeoffItem.autocontinue = 1;
        takeoffItem.param4 = Float.NaN;
        takeoffItem.z = 50;  //takeoffItem.x = 400808070;//takeoffItem.y = 1165726010;
        takeoffItem.mission_type = MAV_MISSION_TYPE.MAV_MISSION_TYPE_MISSION;
        System.out.println("takeoff--->" + takeoffItem);

        // log.info("-------takeoff:{}-------", new Gson().toJson(takeoffItem));
        MAVLinkPacket takeoffPacket = takeoffItem.pack();
        bytes.add(takeoffPacket.encodePacket());

        vo.getMissionVo().getPoints().forEach(point -> {

            // waypoint
            /** target_system: 50, target_component: 190, command: WAYPOINT, mission_type: MISSION,
             // seq: 1, frame: GLOBAL, current: 0, autocontinue : 1, param1: 0, param2: 0, param3: 0,
             // param4: NaN, x: 400810000, y: 1165726312, z: 50 */
            msg_mission_item_int msgMissionItemInt1 = new msg_mission_item_int();
            msgMissionItemInt1.command = MAV_CMD.MAV_CMD_NAV_WAYPOINT;
            msgMissionItemInt1.sysid = vo.getSysId();
            msgMissionItemInt1.compid = vo.getComId();
            msgMissionItemInt1.target_system = (short) vo.getSysId();
            msgMissionItemInt1.target_component = 190;
            msgMissionItemInt1.seq = seq.getAndIncrement();
            msgMissionItemInt1.frame = point.getAltType() == AltTypeEnum.RELATIVE ? (short) MAV_FRAME.MAV_FRAME_LOCAL_NED : (short) MAV_FRAME.MAV_FRAME_GLOBAL;
            msgMissionItemInt1.current = 0;
            msgMissionItemInt1.autocontinue = 1;
            msgMissionItemInt1.y = (int) (point.getLon() * 10000000);
            msgMissionItemInt1.x = (int) (point.getLat() * 10000000);
            msgMissionItemInt1.z = point.getAltitude();
            msgMissionItemInt1.param4 = Float.NaN;
            //msgMissionItemInt1.param4 = 0.5f; //机头朝向的度数(没有值就传NaN)
            // msgMissionItemInt.param1 = 5; // 到点停留时间
            msgMissionItemInt1.mission_type = MAV_MISSION_TYPE.MAV_MISSION_TYPE_MISSION;
            System.out.println("waypoint--->" + msgMissionItemInt1);
            //log.info("-------waypoint:{}-------", new Gson().toJson(msgMissionItemInt1));
            byte[] byte1 = msgMissionItemInt1.pack().encodePacket();
            bytes.add(byte1);

            //action
            /**
             *
             * target_system: 50, target_component: 190, command: MAV_CMD_DO_RISING_MODE_POINT_ACTION,
             * mission_type: MISSION, seq: 2, frame: MISSION, current: 0, autocontinue : 1, param1: 3,
             * param2: 0, param3: NaN, param4: NaN, x: 0, y: 0, z: NaN
             * */
            msg_mission_item_int msgMissionItemInt2 = new msg_mission_item_int();
            msgMissionItemInt2.command = MAV_CMD.MAV_CMD_DO_RISING_MODE_POINT_ACTION;
            msgMissionItemInt2.sysid = vo.getSysId();
            msgMissionItemInt2.compid = vo.getComId();
            msgMissionItemInt2.target_system = (short) vo.getSysId();
            msgMissionItemInt2.target_component = 190;
            msgMissionItemInt2.seq = seq.getAndIncrement();
            msgMissionItemInt2.frame = MAV_FRAME.MAV_FRAME_MISSION;
            msgMissionItemInt2.current = 0;
            msgMissionItemInt2.autocontinue = 1;
            msgMissionItemInt2.param1 = 3; // 变高方式：1-优先变高，2-到点变高，3-安全变高，4-均匀变高，5-仿地变高
            msgMissionItemInt2.param2 = 0; // 1-投放，2-喷洒，3-拍摄，4-环绕
            msgMissionItemInt2.param3 = Float.NaN;
            msgMissionItemInt2.param4 = Float.NaN;
            msgMissionItemInt2.z = Float.NaN;

            // msgMissionItemInt2.param3 = 16; // 当param2=1时候，param3为投放通道（1~16），当param2=2喷洒时候，param3为喷洒通道（1~16）
            // msgMissionItemInt2.param4 = 143; // 当param2=4时候，param4为环绕半径，
            // msgMissionItemInt2.x = 143; // 当param2=4时候，x为环绕时间
            // msgMissionItemInt2.y = 10; // 当param2=4时候，y为环绕圈数量
            msgMissionItemInt2.mission_type = MAV_MISSION_TYPE.MAV_MISSION_TYPE_MISSION;
            // log.info("-------point action:{}-------", new Gson().toJson(msgMissionItemInt2));
            System.out.println("point action--->" + msgMissionItemInt2);
            byte[] byte2 = msgMissionItemInt2.pack().encodePacket();
            bytes.add(byte2);

            /**
             *  target_system: 50, target_component: 190, command: DO_CHANGE_SPEED, mission_type: MISSION,
             *  seq: 3, frame: MISSION, current: 0, autocontinue : 1, param1: 1, param2: 5,
             *  param3: -1, param4: 0, x: 0, y: 0, z: 0
             */
            msg_mission_item_int msgMissionItemInt3 = new msg_mission_item_int();
            msgMissionItemInt3.command = MAV_CMD.MAV_CMD_DO_CHANGE_SPEED;
            msgMissionItemInt3.sysid = vo.getSysId();
            msgMissionItemInt3.compid = vo.getComId();
            msgMissionItemInt3.target_system = (short) vo.getSysId();
            msgMissionItemInt3.target_component = 190;
            msgMissionItemInt3.seq = seq.getAndIncrement();
            msgMissionItemInt3.frame = MAV_FRAME.MAV_FRAME_MISSION;
            msgMissionItemInt3.current = 0;
            msgMissionItemInt3.autocontinue = 1;
            msgMissionItemInt3.param1 = 1;
            msgMissionItemInt3.param2 = point.getSpeed(); // 速度值
            msgMissionItemInt3.param3 = -1;
            msgMissionItemInt3.mission_type = MAV_MISSION_TYPE.MAV_MISSION_TYPE_MISSION;
            // log.info("-------change speed:{}-------", new Gson().toJson(msgMissionItemInt3));
            System.out.println("change speed--->" + msgMissionItemInt3);
            byte[] byte3 = msgMissionItemInt3.pack().encodePacket();
            bytes.add(byte3);

        });


//        msg_mission_item_int returnLaunch = new msg_mission_item_int();
//        returnLaunch.command = MAV_CMD.MAV_CMD_NAV_RETURN_TO_LAUNCH;
//        returnLaunch.sysid = vo.getSysId();
//        returnLaunch.compid = vo.getComId();
//        returnLaunch.target_system = (short) vo.getSysId();
//        returnLaunch.target_component = 190;
//        returnLaunch.frame = MAV_FRAME.MAV_FRAME_GLOBAL;
//        returnLaunch.seq = seq.getAndIncrement();
//        returnLaunch.current = 0;
//        returnLaunch.autocontinue = 1;
        //log.info("-------returnLaunch:{}-------", new Gson().toJson(returnLaunch));
//        MAVLinkPacket returnLaunchPacket = returnLaunch.pack();
//        bytes.add(returnLaunchPacket.encodePacket());

        return bytes;
    }

    // 设定吊舱偏航俯仰角
    public byte[] doMountControl() {
        msg_mission_item_int msgMissionItemInt = new msg_mission_item_int();
        msgMissionItemInt.command = MAV_CMD.MAV_CMD_DO_MOUNT_CONTROL;
        msgMissionItemInt.param1 = -5.5f;//吊舱俯仰角，5.5要写成-5.5
        msgMissionItemInt.param3 = 6.6f; // 吊舱偏航角
        msgMissionItemInt.z = 2;
        return msgMissionItemInt.pack().encodePacket();
    }

    // 设定吊舱变焦
    public byte[] setCameraZoom() {
        msg_mission_item_int msgMissionItemInt = new msg_mission_item_int();
        msgMissionItemInt.command = MAV_CMD.MAV_CMD_SET_CAMERA_ZOOM;
        msgMissionItemInt.param2 = 7.7f;//设定吊舱变焦
        return msgMissionItemInt.pack().encodePacket();
    }

    // 设定相机模型
    public byte[] setCameraMode() {
        msg_mission_item_int msgMissionItemInt = new msg_mission_item_int();
        msgMissionItemInt.command = MAV_CMD.MAV_CMD_SET_CAMERA_MODE;
        return msgMissionItemInt.pack().encodePacket();
    }

    // 定时拍照
    public byte[] imageStartCapture() {
        msg_mission_item_int msgMissionItemInt = new msg_mission_item_int();
        //
        msgMissionItemInt.command = MAV_CMD.MAV_CMD_IMAGE_START_CAPTURE;
        // msgMissionItemInt.param3 = 1;//
        msgMissionItemInt.param2 = 11f;//设置相机模式为定时拍照时候的定时时间间隔
        return msgMissionItemInt.pack().encodePacket();
    }

    // 定距拍照
    public byte[] setCamTrigDist() {
        msg_mission_item_int msgMissionItemInt = new msg_mission_item_int();
        //
        msgMissionItemInt.command = MAV_CMD.MAV_CMD_DO_SET_CAM_TRIGG_DIST;
        msgMissionItemInt.param1 = 8.8f;//定距拍照。拍照距离
        msgMissionItemInt.param3 = 1;
        return msgMissionItemInt.pack().encodePacket();
    }

    // 停止拍照
    public byte[] stopCapture() {
        msg_mission_item_int msgMissionItemInt = new msg_mission_item_int();
        //
        msgMissionItemInt.command = MAV_CMD.MAV_CMD_IMAGE_STOP_CAPTURE;
        return msgMissionItemInt.pack().encodePacket();
    }

    // 开始录影
    public byte[] startVideo() {
        msg_mission_item_int msgMissionItemInt = new msg_mission_item_int();
        //
        msgMissionItemInt.command = MAV_CMD.MAV_CMD_VIDEO_START_CAPTURE;
        return msgMissionItemInt.pack().encodePacket();
    }

    // 停止录影
    public byte[] stopVideo() {
        msg_mission_item_int msgMissionItemInt = new msg_mission_item_int();
        //
        msgMissionItemInt.command = MAV_CMD.MAV_CMD_VIDEO_STOP_CAPTURE;
        return msgMissionItemInt.pack().encodePacket();
    }


}



